//
// Created by 86156 on 25-7-8.
//

#include "gyro_driver.h"
#include "cmsis_os2.h"
#include "control_driver.h"
#include "usart.h"
#include "view_driver.h"

uint8_t Gyro_Buffer[Gyro_BufferSize];
JY60_t JY60,JY60_Last;

uint16_t flag2=0;
uint8_t JY60_InitFlag = -1;

void JY60_Init(){
    HAL_UARTEx_ReceiveToIdle_DMA(&huart2, Gyro_Buffer, Gyro_BufferSize);
    __HAL_DMA_DISABLE_IT(huart2.hdmarx, DMA_IT_HT); // 禁用中断，保证稳定完整传输
}

void JY60_Decode() {
    // if (Gyro_Buffer[0]==0x55) {
    JY60_Last = JY60;

    JY60.Ax =(short)((short)((short)Gyro_Buffer[3]<<8|Gyro_Buffer[2]))*16*constant_g /32768.f;
    JY60.Ay = (short)((short )((short)Gyro_Buffer[5]<<8|Gyro_Buffer[4]))*16*constant_g /32768.f;
    JY60.Az = (short) ((short )((short)Gyro_Buffer[7]<<8|Gyro_Buffer[3]))*16*constant_g /32768.f;
    JY60.Tem=(short)((short) Gyro_Buffer[ 8] |  (short) Gyro_Buffer[ 9] << 8) /32768*96.38+36.53;

    JY60.Wx = (short) ((int16_t) Gyro_Buffer[13] | (int16_t) Gyro_Buffer[14] << 8) * 2000 / 32768.f;
    JY60.Wy = (short) ((int16_t) Gyro_Buffer[15] | (int16_t) Gyro_Buffer[16] << 8) * 2000 / 32768.f;
    JY60.Wz = (short) ((int16_t) Gyro_Buffer[17] | (int16_t) Gyro_Buffer[18] << 8) * 2000 / 32768.f;

    JY60.Roll =  (short)  ((short) Gyro_Buffer[25] << 8 | (short) Gyro_Buffer[24]) * 180 / 32768.f;
    JY60.Pitch = (short) ((short) Gyro_Buffer[27] << 8 | (short) Gyro_Buffer[26])* 180 / 32768.f ;
    JY60.Yaw =   (short) ((short) Gyro_Buffer[29] << 8 | (short) Gyro_Buffer[28] )* 180 / 32768.f;

    if (JY60_InitFlag == 0) {
        JY60.Roll_Init = JY60.Roll;
        JY60.Yaw_Init = JY60.Yaw;
        JY60_InitFlag = 1;
    }

    JY60.Yaw_Diff = JY60.Yaw - JY60_Last.Yaw;
    if(JY60.Yaw_Diff >  180){JY60.CC_YAW = JY60.CC_YAW - 1;}
    if(JY60.Yaw_Diff < -180){JY60.CC_YAW = JY60.CC_YAW + 1;}
    JY60.Yaw_Total = 360 * (float)JY60.CC_YAW + JY60.Yaw - JY60.Yaw_Init;

    JY60.Rol_Diff = JY60.Roll - JY60_Last.Roll;
    if(JY60.Rol_Diff >  180){JY60.CC_ROL = JY60.CC_ROL - 1;}
    if(JY60.Rol_Diff < -180){JY60.CC_ROL = JY60.CC_ROL + 1;}
    JY60.Rol_Total = 360 * (float)JY60.CC_ROL + JY60.Roll - JY60.Roll_Init;

    Angle_Measure.pit=JY60.Pitch;
    Angle_Measure.rol=JY60.Rol_Total;
    Angle_Measure.yaw=JY60.Yaw_Total;

    // }
}

extern osSemaphoreId_t RcSemBinaryHandle;
extern osSemaphoreId_t GyroSemBinaryHandle;
extern osSemaphoreId_t ViewSemBinaryHandle;

/**
 * @brief 串口空闲中断
 * @param huart UART_Handle
 * @param Size DataSize
 */
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size) {
    if(huart->Instance == huart2.Instance)
    {
        osSemaphoreRelease(GyroSemBinaryHandle);
    }
    else if(huart->Instance == huart1.Instance) {
        osSemaphoreRelease(ViewSemBinaryHandle);
    }
    else if (huart->Instance == huart3.Instance) {
        osSemaphoreRelease(RcSemBinaryHandle);
    }
}

void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) {
    flag2++;
}
